package com.zhdl.modules.autopick;


import cn.hutool.core.util.HexUtil;
import com.google.common.eventbus.Subscribe;
import com.google.common.primitives.Bytes;
import com.project.command.instruction.strategy.process.data.AfterStrategyParam;
import com.project.common.util.ByteHelper;
import com.project.common.util.DelayUtil;
import com.zhdl.common.eventbus.center.EventBusCenter;
import com.zhdl.modules.autopick.entity.Cassette;
import com.zhdl.modules.autopick.entity.CassettePosition;
import com.zhdl.modules.autopick.entity.Sensor;
import com.zhdl.modules.autopick.service.CameraService;
import com.zhdl.modules.autopick.service.CmdHandleService;
import com.zhdl.modules.autopick.service.MotorActionService;
import com.zhdl.modules.autopick.service.RobotActionService;
import com.zhdl.modules.autopick.web.request.MemDevice;
import com.zhdl.modules.autopick.web.response.SseEmitterResultVO;
import com.zhdl.modules.tools.EventBusTag;
import com.zhdl.modules.tools.InitCassette;
import com.zhdl.modules.tools.PublicTools;
import com.zhdl.network.modbus.dto.ModbusDto;
import com.zhdl.network.service.MotorService;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.scheduling.concurrent.ThreadPoolTaskScheduler;
import org.springframework.stereotype.Component;

import javax.annotation.Resource;
import java.math.BigInteger;
import java.time.Instant;
import java.util.*;
import java.util.concurrent.ScheduledFuture;

@Slf4j
@Component
public class ProcessControlSignal {

    /**
     * 是否在调试状态
     */
    public static boolean isTest = false;
    /**
     * 主进度 流程进度 - 机器人
     * 分支进度 流程进度 - 电机
     */
    public int processR = -1, processM = -1;
    /**
     * 是否初始化
     * 是否初始化完成
     */
    public boolean isInit = false, isInitFinish = false;
    /**
     * 当前流程是否在运行 - 默认未运行
     * true 运行 false 未运行
     */
    public boolean isRunning = false;
    /**
     * 是否初始化完成 --- 机器人
     */
    public boolean robotInit = false;
    /**
     * 料盒初始化
     */
    public List<Cassette> cassetteList;
    /**
     * 当前需要添加药物的料斗
     */
    public Cassette current;
    /**
     * pcb板的查询存储
     */
    public List<Byte> pcbStatusDto02 = new ArrayList<>(), pcbStatusDto01 = new ArrayList<>();
    /**
     * 机器人状态 - 需要谁去使用
     */
    public String robotStatusUser = "";
    /**
     * 电机原点状态 - 是否在原点
     */
    public String motorHomeStatus = "";
    /**
     * 临时状态存储--机器人状态
     */
    public String statusM20 = "", statusM21 = "", statusM22 = "";
    public String statusM120 = "", statusM121 = "", statusM122 = "", statusM123 = "", statusM124 = "";
    /**
     * 电机-当前电气位置
     */
    public int motorCurrentLocation = -1;
    /**
     * 电机-需要移动到的位置
     * 注：第一段第五段是根据当时赋值，二-四为固定距离
     */
    public int motorOneArriveLocation = -1, motorFiveArriveLocation = -1;
    /**
     * 临时状态存储--电机状态
     * Step为当前步数是否到位 -- 机械位置
     */
    @Autowired
    public ThreadPoolTaskScheduler taskScheduler1;
    public MemDevice memDevice = new MemDevice();
    /**
     * 首页信息
     */
    public SseEmitterResultVO sseResp = new SseEmitterResultVO();
    /**
     * 机器人-当前世界坐标
     */
    int[] robotLocationNow = new int[6];
    /**
     * 电机-脉冲对照表
     */
    CassettePosition cassettePosition = new CassettePosition();
    /**
     * 设置 当前位移偏差 -- 当原点不为0的时候
     */
    int differentLocation = 0;
    /**
     * ScheduledFuture 存储集合
     */
    Map<String, ScheduledFuture<?>> mapSF = new HashMap<>();
    @Resource
    private CameraService cameraService;
    @Resource
    private CmdHandleService cmdHandleService;
    @Resource
    private RobotActionService robotActionService;
    @Resource
    private MotorActionService motorActionService;
    @Resource
    private MotorService motorService;

    /**
     * 将byte转换为一个长度为8的byte数组，数组每个值代表bit
     */
    public static Byte[] getBitArray(byte b) {
        Byte[] array = new Byte[8];
        for (int i = 7; i >= 0; i--) {
            array[i] = (byte) (b & 1);
            b = (byte) (b >> 1);
        }
        return array;
    }

    /**
     * 根据传感器Id 获取料盒Id
     *
     * @param sensorId 传感器Id
     * @return 料盒Id
     */
    public int getCassetteIdBySensorId(int sensorId) {

        int cassetteId = -1;

        for (Cassette cassette : cassetteList) {
            for (Sensor sensor : cassette.getSensorList()) {
                if (sensor.getId() == sensorId) {
                    cassetteId = cassette.getId();
                    break;
                }
            }
        }

        return cassetteId;
    }
    /*
     * ----------------------------------测试结束------------------------------------------------
     */

    /*
     * ----------------------------------初始化------------------------------------------------
     */

    /*
     * ----------------------------------测试------------------------------------------------
     */
    @Subscribe
    public void eventApi(EventBusTag event) {
        String tag = event.getTag();
        switch (tag) {
            case "robotInit" -> {
                processR = 2;
                nextIntRobot();
            }
            case "robotPick" -> {
                processR = 4;
                nextIntRobot();
            }
            case "motorLocation" -> {
                motorService.motorWriteRun();
            }
        }
    }

    /**
     * 初始化参数
     */
    private void release() {
        processR = -1;
        processM = -1;
        isRunning = false;
        current = null;
    }

    /**
     * 结束流程-判断是否全部完成
     */
    public void processFinish() {
        if (processR >= 13 && processM >= 12) {
            release();
            log.info("流程全部完成");
        }
    }

    /**
     * 从数据库加载设备到内存 - 赋值初始化，不至于为空
     */
    public void BuildMemDev() {
        for (int i = 0; i < 16; i++) {
            pcbStatusDto01.add((byte) 0);
        }
        for (int i = 0; i < 48; i++) {
            pcbStatusDto02.add((byte) 1);
        }
        BuildMemDev2();
    }

    /**
     * 从数据库加载设备到内存 - 将读取到的加载到内存
     * 000000011000011000001000011000000000000000000000
     */
    public void BuildMemDev2() {
        byte[] array01 = Bytes.toArray(pcbStatusDto01);
        byte[] array02 = Bytes.toArray(pcbStatusDto02);
        StringBuffer string01 = new StringBuffer();
        StringBuffer string02 = new StringBuffer();
        for (byte b : array01) {
            string01.append(b);
        }
        for (byte b : array02) {
            string02.append(b);
        }
        log.info("重点关注01+++++++++++++++++++++++" + string01);
        log.info("重点关注02+++++++++++++++++++++++" + string02);
        memDevice.setCoilIO(string01.toString());
        memDevice.setSensorIO(string02.toString());
        sseResp.setMemDevice(memDevice);
    }

    /*
     * ----------------------------------初始化结束------------------------------------------------
     */

    /**
     * 初始化设备 加载到内存
     */
    public void init() {
        /*
            初始化料盒
         */
        cassetteList = InitCassette.cassetteInit();

        /*
            初始化时-注册广播用户
         */
        EventBusCenter.getEventBus().register(this);
        System.out.println(cassetteList);
        /*
            一直查询pcb状态
         */
        taskScheduler1.scheduleWithFixedDelay(new Runnable() {
            @Override
            public void run() {
                cmdHandleService.pcbReadInfo();
            }
        }, 1000);
        /*
            一直查询Motor当前位置
         */
        taskScheduler1.scheduleWithFixedDelay(new Runnable() {
            @Override
            public void run() {
                cmdHandleService.motorLocation();
            }
        }, 1000);

        /*
            写入空压机开启
         */
        cmdHandleService.ioWriteCoil(0, "FF00");

        /*
            写入夹爪闭合
         */
        cmdHandleService.ioWriteCoil(1, "FF00");

        /*
            写入压板上合
         */
        cmdHandleService.ioWriteCoil(3, "0000");

        /*
            拉杆回收
         */
        cmdHandleService.ioWriteDoubleCoil(6, false);

        if (!isInit && !isInitFinish) {
            //开始初始化 - 设置为True
            isInit = true;
            //初始化-电机找原点
            processM = 0;
            nextIntMotor();

            log.info("-电机进行初始化");
        }

        if (!robotInit) {
            processR = 2;
            nextIntRobot();
        }

    }

    /**
     * 广播接收 串口通讯的数据
     *
     * @param event 解析后的数据
     */
    @Subscribe
    public void eventReceiver(AfterStrategyParam event) {

        if (event.getPortKey().equals("192.168.1.108:502")) {    // 远程IO相关指令
            //if (event.getPortKey().equals("127.0.0.1:502")) {    // 远程IO相关指令

            List<ModbusDto> list = event.getNewData();

            //此处接收串口或网口客户端回应代码,并处理
            ModbusDto pcbDto = list.get(0);
            String instruction = event.getInstruction().replaceAll(" ", "");
            //功能码02 读取48个离散量数目
            String statusInstruction02 = PublicTools.getString("cmd", "pcbReadDiscreteSignal");
            //功能码01 读取16个离散量数目
            String statusInstruction01 = PublicTools.getString("cmd", "pcbReadCoilSignal");

            //单独读取当前地址
            String address = instruction.substring(5, 8);

            //查询pcb当前的状态 000100000006650200000030
            if (instruction.equals(statusInstruction02)) {
                byte[] unitBuff = pcbDto.getUnitBuff();
                List<Byte> list1 = new ArrayList<>();
                for (byte b : unitBuff) {
                    Byte[] bitArray = getBitArray(b);
                    List<Byte> list3 = Arrays.asList(bitArray);
                    Collections.reverse(list3);
                    list1.addAll(list3);
//                    List<Byte> list2 = new ArrayList<>();
//                    for (byte b1 : bitArray) {
//                        //每8位 数据高低位处理
//                        list2.add(b1);
//                        if (bitArray.length % 8 == 0) {
//                            Collections.reverse(list2);
//                            for (Byte aByte : list2) {
//                                list1.add(aByte);
//                            }
//                            list2.clear();
//                        }
//                    }
                }

                //判断当前是哪一个料斗
                if (list1.get(8) == 0) {
                    log.info("判断料斗一，报警");
                    processEntryBySensorId(8);
                }
                if (list1.get(14) == 0) {
                    processEntryBySensorId(14);
                    log.info("判断料斗二，报警");
                }
                if (list1.get(20) == 0) {
                    processEntryBySensorId(20);
                    log.info("判断料斗三，报警");
                }
                if (list1.get(26) == 0) {
                    processEntryBySensorId(26);
                    log.info("判断料斗四，报警");
                }

                this.pcbStatusDto02 = list1;

                //将读取的数据加载到内存
                BuildMemDev2();

                //判断哪一位是否打开
                for (int i = 0; i < list1.size(); i++) {
                    Byte b = list1.get(i);
                    if (b == 0) {
                        System.out.println("第" + i + "位，未打开");
                    } else {
                        System.out.println("第" + i + "位，当前打开");
                    }
                }

                /**
                 * 判断流程是否开始
                 */

            } else if (instruction.equals(statusInstruction01)) {
                byte[] unitBuff = pcbDto.getUnitBuff();
                List<Byte> list1 = new ArrayList<>();
                for (byte b : unitBuff) {
                    Byte[] bitArray = getBitArray(b);
                    List<Byte> list3 = Arrays.asList(bitArray);
                    Collections.reverse(list3);
                    list1.addAll(list3);
//                    list1.addAll(Arrays.asList(bitArray));
                }

                //每8位 数据高低位处理

                this.pcbStatusDto01 = list1;

                //将读取的数据加载到内存
                BuildMemDev2();

                //判断哪一位是否打开
                for (int i = 0; i < list1.size(); i++) {
                    Byte b = list1.get(i);
                    if (b == 0) {
                        System.out.println("第" + i + "位，未打开");
                    } else {
                        System.out.println("第" + i + "位，当前打开");
                    }
                }
            } else {
                log.info("未匹配到02 or 01的功能码，当前的功能码为:" + event.getPortKey());
            }

        } else if (event.getPortKey().equals("1")) { //motor 串口相关指令

            List<ModbusDto> list = event.getNewData();
            //此处接收串口或网口客户端回应代码,并处理
            ModbusDto modbusDto = list.get(0);
            String instruction = event.getInstruction();
            instruction = instruction.replaceAll(" ", "");
            String statusInstruction = "01030B07000277EE";

            //单独读取当前地址
            String address = instruction.substring(5, 8);

            //查询电机状态 0B07 010305240001C4CD
            if (instruction.equals(statusInstruction)) {
                //查询是否功能码为3

                if (modbusDto.getCmdType() == 3) {
                    //获取响应数据
                    byte[] fullBuff = modbusDto.getUnitBuff();
                    byte[] mPosition = new byte[4];

                    mPosition[2] = fullBuff[0];
                    mPosition[3] = fullBuff[1];
                    mPosition[0] = fullBuff[2];
                    mPosition[1] = fullBuff[3];

                    String s = ByteHelper.bytesArrayToHexString(mPosition);
                    BigInteger bigInteger = new BigInteger(s, 16);
                    motorCurrentLocation = bigInteger.intValue();
                    //motorCurrentLocation = ByteHelper.byteArrayToInt(mPosition);
                }
            }//判断 如果指令 等于 下面指令(查询当前是否回到原点)
            else if (instruction.equals("010330040001CACB")) {
                //获取响应数据
                byte[] unitBuff = modbusDto.getUnitBuff();
                String s = HexUtil.encodeHexStr(unitBuff);
                log.info("读取状态+++++++++++++++" + s);
                // 96 没有回到原点 224 回到原点完成
                if (s.equals("00e0")) {
                    motorHomeStatus = "1";
                    log.info("当前电机回到原点");
                } else {
                    motorHomeStatus = "0";
                    log.info("当前电机未回到原点");
                }
            } else {
                log.info("当前指令为:" + instruction + ",没有获取到对应的指令处理");
            }

        } else if (event.getPortKey().equals("192.168.4.4:502")) { //机器人相关信息
            List<ModbusDto> list = event.getNewData();
            //此处接收串口或网口客户端回应代码,并处理
            ModbusDto modbusDto = list.get(0);
            String instruction = event.getInstruction().replaceAll(" ", "");
            String statusInstruction = PublicTools.getString("cmd", "robotReadArmStatus");

            /*
                M10	    等待抓料信号	        M20	    抓料完成信号
                M11	    等待放料信号	        M21	    放料完成信号
                M12	    等待回原点信号	        M22	    回原点完成信号
                M110    等待夹料气缸松开到位	M120	通知夹料气缸松开
                M111	等待夹料气缸夹紧到位	M121	通知夹料气缸夹紧
                M112	等待推袋气缸下推到位	M122	通知推袋气缸下推
                M113	等待推袋气缸上回到位	M123	通知推袋气缸上回
                M114	等待抖料动作完成信号	M124	通知抖料动作开始
                A0-BF                       C0-DF
             */
            if (modbusDto.getCmdType() == 0x01) {
                String robotStatusM00 = instruction.substring(16, 20);
                switch (robotStatusM00) {
                    //等待抓料信号 M20-M22
                    case "00A8" -> {
                        statusM20 = ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff());
                    }
                    case "00A9" -> {
                        statusM21 = ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff());
                    }
                    case "00AA" -> {
                        statusM22 = ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff());
                    }
                    //M120-M124
                    case "00C8" -> {
                        statusM120 = ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff());
                    }
                    case "00C9" -> {
                        statusM121 = ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff());
                    }
                    case "00CA" -> {
                        statusM122 = ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff());
                    }
                    case "00CB" -> {
                        statusM123 = ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff());
                    }
                    case "00CC" -> {
                        statusM124 = ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff());
                    }

                }
            }

            if (instruction.equals(statusInstruction)) {
                //获取响应数据
                byte[] unitBuff = modbusDto.getUnitBuff();
                String s = HexUtil.encodeHexStr(unitBuff);
                if (robotStatusUser.equals("robotArmInit")) {

                    for (int i = 0; i < s.length(); i++) {
                        if (i % 8 == 0) {
                            String substring = s.substring(i, i + 8);
                            BigInteger bigInteger = new BigInteger(substring, 16);
                            int i1 = bigInteger.intValue();
                            robotLocationNow[i / 8] = i1;
                        }
                    }
                    processR = 3;
                    nextIntRobot();
                }

            }
        } else {
            log.info("串口未匹配");
        }

        System.out.println(Instant.now() + "监听者1-->回调1,收到事件：Com" + event.getPortKey() + "，线程号为：" + Thread.currentThread().getName());
    }

                        /*      y版本
                    List<Byte> list10 = new ArrayList<>();
                        List<Byte> list2 = new ArrayList<>();
                        for (byte b1 : bitArray) {
                            //每8位 数据高低位处理
                            list2.add(b1);
                            if (bitArray.length % 8 == 0) {
                                Collections.reverse(list2);
                                for (Byte aByte : list2) {
                                    list10.add(aByte);
                                }
                                list2.clear();
                            }
                        }
                    this.pcbStatusDto10 = list10;
                     */

    /**
     * 加药流程_开始入口
     *
     * @param id 传感器id
     */
    public void processEntryBySensorId(int id) {
        /* 判断电机初始化完成 - 机器人是否初始化完成 */
        if (!isInitFinish) {
            return;
        }
        /*   根据传感器id，判断料盒id   */
        id = getCassetteIdBySensorId(id);
        /*      开始流程      */
        processEntryByCassetteId(id);
    }

    /**
     * 加药流程_开始入口
     *
     * @param id 料盒Id
     */
    public void processEntryByCassetteId(int id) {

        log.info("流程开始了-=-=-------------------------");

        /* 判断初始化是否完成 */
        if (!isInitFinish || !robotInit) {
            log.info("当前初始化状态，motor+" + isInitFinish + ",robot+" + robotInit);
            return;
        }

        /* 判断当前是否有流程正在进行 */
        if (isRunning) {
            return;
        }
        isRunning = true;

        /* 判断是哪一个传感器报警 */
        for (Cassette cassette : cassetteList) {
            if (cassette.getId() == id) {
                current = cassette;
                break;
            }
        }
        log.info("当前料盒id为:" + current.getId() + " ++++++++++++++++++++++++++++");
        /* 判断转运料斗是否有料 */
        //            String value = CacheUtil.getSingleTon().getValue("isEmpty");
        String value = "true";

        //如果为1 则有料
        if (value.equals("false")) {
            //直接开始电机运动
            processM = 1;
            nextIntMotor();
        } else {    //如果为0，则没有料

            //判断机器人抓手是否有料 0 无
//            if (pcbStatusDto02.get(0) == 0) {
            if (true) {
                log.info("开始控制啦!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
                /* 无负重情况 设置抓料位置 设置抓料上方位置 读M121控制夹具夹紧 写M10 读M20 后续到负重流程 */
                /* 调用相机拍照 */
                cameraService.takePhoto();
                DelayUtil.delayedExecution(2000, new Runnable() {
                    public void run() {
                        /* 计算出坐标 */
                        cameraService.getComputeResult();
                        DelayUtil.delayedExecution(2000, new Runnable() {
                            public void run() {
                                processR = 4;
                                nextIntRobot();
//                                processM = 1;
//                                nextIntMotor();
                            }
                        });
                    }
                });
            } else {    //判断机器人抓手是否有料 1 有

                /* 有负重写M11 同时读M124 控制气缸抖动 完成后M114 继续读M21 完成后 电机流程启动 */
                //查看机器人状态
                if (robotInit) {
                    processR = 6;
                    nextIntRobot();
                }
            }
        }

    }

    /**
     * 机器人流程-主流程
     */
    public void nextIntRobot() {

        /*
            判断是否是在调试状态 如果在就退出流程
         */
        if (isTest) {
            return;
        }

        processR++;
        switch (processR) {
            case 1 -> {
                //默认位，视觉拍照
                nextIntRobot();
                log.info("视觉拍照");
            }
            case 2 -> {
                //默认位，计算视觉坐标
                nextIntRobot();
                log.info("计算视觉坐标");
            }
            case 3 -> { //读取机器人世界坐标
                /* 当前状态的使用者 机器人是否初始化 */
                robotStatusUser = "robotArmInit";
                robotActionService.getRobotCoordinate();
            }
            case 4 -> { //机器人-机械臂 初始化
                //1. 需要先给机器人 上电上抬位置
                //2. 然后等待M12信号
                robotActionService.robotArmInit(robotLocationNow);
                //3. 读取M22是否到达原点
                //判断机器人是否初始化完成
                delayStatusM22();
            }
            case 5 -> { //机器人-机械臂 移动到料袋准备抓料
                //1. 发送给机器人两个坐标 ==> 并且写入M10 开始抓料
                robotActionService.robotRunToDrug(
                        new int[]{2201352, -119700, 1096243},
                        new int[]{2201352, -119700, 1096243}
                );
                // 1.5 提前张开机械爪
                cmdHandleService.ioWriteCoil(1, "0000");
                //2. 读 M121 等待通知夹料气缸夹紧
                delayM121();
            }
            case 6 -> {// 机器人-抓具夹紧
                //1. 写入气缸夹紧信号 ->放料信号
                robotActionService.robotWriteArmPick();
                //2. 读取M20信号抓料完成信号
                delayStatusM20();
            }
            case 7 -> {//等待抖料信号 M124
                //写入放料信号 M11
                robotActionService.writeM11();
                //2. 读取M124信号，等待抖料
                delayStatusM124();
            }
            case 8 -> { //机器人-抓具抖动
                robotActionService.robotArmShake();
                //等待放料完成信号 M21
                delayStatusM21();
                //CacheUtil.getSingleTon().putValue("isEmpty", "false");
            }
            case 9 -> {
                processM = 1;
                //开始电机流程分化
                nextIntMotor();
                //通知夹料气缸松开
                delayStatusM120();
            }
            case 10 -> { // 机器人-夹具松开
                robotActionService.robotArmDownBag();
                //等待气缸下推动作
                delayStatusM122();
            }
            case 11 -> {//气缸下推
                //----
                robotActionService.robotArmAirDown();
                //等待气缸上回信号
                delayStatusM123();
            }
            case 12 -> {//气缸上回动作
                getSensorStatus(1, 0, this::nextIntRobot, 2000);
            }
            case 13 -> {
                robotActionService.robotArmAirUp();
                DelayUtil.delayedExecution(1000, new Runnable() {
                    @Override
                    public void run() {
                        cmdHandleService.ioWriteCoil(1, "FF00");
                    }
                });
                log.info("机器人流程结束");
                processFinish();
            }
            default -> log.info("电机流程-出现异常");
        }
    }

    /**
     * 电机流程-分化流程
     */
    public void nextIntMotor() {
        processM++;
        switch (processM) {
            case 1 -> { //电机找原点
                //1. 开机找原点
                motorActionService.motorInitZero();
                //2. 读取电机当前状态 - 初始化完成 H17.41
                delayMotorInitFinish(new StatusCallback() {
                    @Override
                    public void statusCallback() {
                        isInitFinish = true;
                        log.info("初始化完成");
                    }
                });
            }
            case 2 -> { //电机移动-到料盒位置 - 推杆位置
                differentLocation = motorCurrentLocation;
                //注意：-------------以下设置，均为多端中的第一段---------------
                //1. 可以通过料盒位置，固定 第一段走的距离
                if (current.getId() == 0) {
                    // 设置第一个料盒的第一段要走的距离
                    motorOneArriveLocation = 334375;
                    //设置第一个料盒对应的脉冲数
                    cassettePosition.setCasOpenLid(481250);
//                    cassettePosition.setCasOpenLid(451250);
                    cassettePosition.setCasRunCloseLid(31250);
                    cassettePosition.setCasCloseLid(-343750);
//                    cassettePosition.setCasCloseLid(-313750);
                    cassettePosition.setCasBackHome(-503125);
                    //2. 走到第一个料盒
                    motorActionService.motorOneStep(motorOneArriveLocation);
                    log.info("判断为第一个料盒");
                } else if (current.getId() == 1) {
                    // 设置第二个料盒的第一段要走的距离
                    motorOneArriveLocation = 2906250;
                    //设置第二个料盒对应的脉冲数
                    cassettePosition.setCasOpenLid(500000);
//                    cassettePosition.setCasOpenLid(470000);
                    cassettePosition.setCasRunCloseLid(31250);
                    cassettePosition.setCasCloseLid(-346875);
//                    cassettePosition.setCasCloseLid(-316875);
                    cassettePosition.setCasBackHome(-3090625);
                    //2. 走到第二个料盒
                    motorActionService.motorOneStep(motorOneArriveLocation);
                    log.info("判断为第二个料盒");
                } else if (current.getId() == 2) {
                    // 设置第三个料盒的第一段要走的距离
                    motorOneArriveLocation = 5471875;
                    //设置第三个料盒对应的脉冲数
                    cassettePosition.setCasOpenLid(456250);
//                    cassettePosition.setCasOpenLid(426250);
                    cassettePosition.setCasRunCloseLid(31250);
                    cassettePosition.setCasCloseLid(-309375);
//                    cassettePosition.setCasCloseLid(-279375);
                    cassettePosition.setCasBackHome(-5650000);
                    //2. 走到第三个料盒
                    motorActionService.motorOneStep(motorOneArriveLocation);
                    log.info("判断为第三个料盒");
                } else if (current.getId() == 3) {
                    // 设置第四个料盒的第一段要走的距离
                    motorOneArriveLocation = 8031250;
                    //设置第四个料盒对应的脉冲数
                    cassettePosition.setCasOpenLid(456250);
//                    cassettePosition.setCasOpenLid(426250);
                    cassettePosition.setCasRunCloseLid(25000);
                    cassettePosition.setCasCloseLid(-309375);
//                    cassettePosition.setCasCloseLid(-279375);
                    cassettePosition.setCasBackHome(-8203125);
                    //2. 走到第四个料盒
                    motorActionService.motorOneStep(motorOneArriveLocation);
                    log.info("判断为第四个药盒");
                }
                //然后读取是否到达了位置 -- 电机如果到达位置，则进行打开料盒
                stepComplete(motorOneArriveLocation, this::nextIntMotor);
            }
            case 3 -> { //电机移动-停止运动 放下杆
                //同时在快到的时候放下杆儿
                motorActionService.motorGoOpenBar();
//                DelayUtil.delayedExecution(1000, new Runnable() {
//                    @Override
//                    public void run() {
//                        //读取IO是否 杆儿落下来了
//                        nextIntMotor();
//                    }
//                });
                getSensorStatus(32, 1, this::nextIntMotor, 2000);
            }
            case 4 -> { //电机移动-开始运动 拉杆推开
                differentLocation = motorCurrentLocation;
                //第二段运动
                motorActionService.motorTwoStep(cassettePosition.getCasOpenLid());
                //判断是否料盒打开到位
                stepComplete(cassettePosition.getCasOpenLid(),
                        () -> getSensorStatus(current.getSensorByType("1"), 1,
                                this::nextIntMotor, 2000));

            }
            case 5 -> { //IO控制-收回拉杆，打开电磁阀
                motorActionService.motorThreeStep();
                //读取IO是否 杆儿升起来了/电磁阀是否打开
                //nextIntMotor();
                getSensorStatus(33, 1, this::nextIntMotor, 2000);
//                getSensorStatus(current.getSensorByType("3"), 0, () ->
//                        getSensorStatus(30, 1,
//                                () -> getSensorStatus(31, 0,
//                                        this::nextIntMotor, 2000), 2000), 2000);
            }
            case 6 -> { //关闭电磁阀
                //当电磁阀打开的时候，已经可以判断 转运料斗为空
                //CacheUtil.getSingleTon().putValue("isEmpty", "false");
                //关闭电磁阀
                DelayUtil.delayedExecution(40000, new Runnable() {
                    @Override
                    public void run() {
                        motorActionService.motorFourStep();
                        DelayUtil.delayedExecution(2000, new Runnable() {
                            @Override
                            public void run() {
                                nextIntMotor();
                            }
                        });
                    }
                });
                //判断电磁阀是否关闭
//                DelayUtil.delayedExecution(10000, () -> {
//                    getSensorStatus(30, 0,
//                            () -> getSensorStatus(31, 0, this::nextIntMotor,
//                                    2000), 2000);
//                });

            }
            case 7 -> { //电机移动-GO合盖补偿

                differentLocation = motorCurrentLocation;

                //向前移动到可以放下杆的位置
                motorActionService.motorFiveStep(cassettePosition.getCasRunCloseLid());

                //differentLocation = differentLocation+30000;

                //读取电机是否移动到位 是否可以放杆儿
                stepComplete(cassettePosition.getCasRunCloseLid(), this::nextIntMotor);
            }
            case 8 -> { //电机移动-停止运动 放下杆
                //IO控制 放下杆儿
                motorActionService.motorBackOpenBar();
//                DelayUtil.delayedExecution(1000, new Runnable() {
//                    @Override
//                    public void run() {
//                        //读取IO 杆儿是否放下来了
//                        nextIntMotor();
//                    }
//                });
                getSensorStatus(32, 1, this::nextIntMotor, 2000);
            }
            case 9 -> { //电机移动-Back合盖补偿

                differentLocation = motorCurrentLocation;

                //电机运动到 合上电机漏斗的位置
                motorActionService.motorSixStep(cassettePosition.getCasCloseLid());

                //读取电机当前位置，判断电机是否到位
                stepComplete(cassettePosition.getCasCloseLid(),
                        () -> getSensorStatus(current.getSensorByType("2"), 1,
                                this::nextIntMotor, 2000));
            }
            case 10 -> {//电机移动-停止运动 抬起杆
                //IO控制 抬起杆儿
                motorActionService.motorBackCloseBar();
//                DelayUtil.delayedExecution(3000, new Runnable() {
//                    @Override
//                    public void run() {
//                        //读取IO 杆儿是否抬起来了
//                        //delayMotorReadCloseBar();
//                        nextIntMotor();
//                    }
//                });
                getSensorStatus(33, 1, this::nextIntMotor, 2000);
            }
            case 11 -> { //电机运动-回到原点

                differentLocation = motorCurrentLocation;

                //注意：-------------以下设置，均为多端中的第五段---------------
                //1. 可以通过料盒位置，固定 第一段走的距离
                motorActionService.motorSevenStep(cassettePosition.getCasBackHome());
                //读取电机当前位置，判断电机是否到位
                stepComplete(cassettePosition.getCasBackHome(), this::nextIntMotor);
                log.info("电机-到达原点");
            }
            case 12 -> {//电机运动-到达原点
                //电机到达原点 设置为 伺服状态
                motorActionService.motorEightStep();
                //读取电机当前位置，判断电机是否到位
                log.info("电机流程结束");
                processFinish();

            }
            default -> log.info("电机流程出现异常");
        }

    }
    /*
     * ---------------------------------机器人状态判断-----------------------------------------
     */

    /**
     * 电机-推杆伸出
     * 判断当前为哪个料盒报警
     */
    public void poleOpen(int id) {
        for (Cassette cassette : cassetteList) {
            for (Sensor sensor : cassette.getSensorList()) {
                if (sensor.getId() == id) {
                    if (sensor.getType().equals("1")) {
                        if (current.getId() == cassette.getId()) {
                            log.info("当前料盒为:" + current.getId() + ",当前传感器为:" + id);
                        }
                    }
                }
            }
        }

    }

    /**
     * 延时获取当前状态-机器人是否抓料完成 M20
     */

    public void delayStatusM20() {
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {

            public void run() {
                if (statusM20.equals("01")) {
                    nextIntRobot();
                    statusM20 = "";
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("M20");
                    robotActionService.writeFalseM(168, false);
                    scheduledFuture00.cancel(false);
                    mapSF.remove("M20");
                } else {
                    robotActionService.getRobotM20();
                }
            }
        }, 100);
        mapSF.put("M20", scheduledFuture);
    }

    /**
     * 延时获取当前状态-机器人是否放料完成 M21
     */

    public void delayStatusM21() {
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {

            public void run() {
                if (statusM21.equals("01")) {
                    nextIntRobot();
                    statusM21 = "";
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("M21");
                    robotActionService.writeFalseM(169, false);
                    scheduledFuture00.cancel(false);
                    mapSF.remove("M21");
                } else {
                    robotActionService.getRobotM21();
                }
            }
        }, 100);
        mapSF.put("M21", scheduledFuture);
    }

    /**
     * 延时获取当前状态-机器人是否初始化完成 M22
     */
    public void delayStatusM22() {

        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {

            public void run() {
                //具体 在 eventbus里面获取结果
                if (statusM22.equals("01")) {
                    statusM22 = "";
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("M22");
                    robotActionService.writeFalseM(170, false);
                    scheduledFuture00.cancel(false);
                    robotInit = true;
                    mapSF.remove("M22");
                } else {
                    robotActionService.getRobotM22();
                }
            }
        }, 100);
        mapSF.put("M22", scheduledFuture);
    }

    /**
     * 延时获取当前状态-气缸向下运动 M122
     */

    public void delayStatusM122() {
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {

            public void run() {
                if (statusM122.equals("01")) {
                    nextIntRobot();
                    statusM122 = "";
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("M122");
                    robotActionService.writeFalseM(202, false);
                    scheduledFuture00.cancel(false);
                    mapSF.remove("M122");
                } else {
                    robotActionService.getRobotM122();
                }
            }
        }, 100);
        mapSF.put("M122", scheduledFuture);
    }

    /**
     * 延时获取当前状态-气缸向上运动 M123
     */

    public void delayStatusM123() {
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {

            public void run() {
                if (statusM123.equals("01")) {
                    nextIntRobot();
                    statusM123 = "";
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("M123");
                    robotActionService.writeFalseM(203, false);
                    scheduledFuture00.cancel(false);
                    mapSF.remove("M123");
                } else {
                    robotActionService.getRobotM123();
                }
            }
        }, 100);
        mapSF.put("M123", scheduledFuture);
    }

    /**
     * 延时获取当前状态-通知夹料气缸松开 M120
     */

    public void delayStatusM120() {
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {

            public void run() {
                if (statusM120.equals("01")) {
                    nextIntRobot();
                    statusM120 = "";
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("M120");
                    robotActionService.writeFalseM(200, false);
                    scheduledFuture00.cancel(false);
                    mapSF.remove("M120");
                } else {
                    robotActionService.getRobotM120();
                }
            }
        }, 100);
        mapSF.put("M120", scheduledFuture);
    }

    /**
     * 延时获取当前状态-等待通知夹料气缸夹紧 M121
     */

    public void delayM121() {
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {

            public void run() {
                if (statusM121.equals("01")) {
                    nextIntRobot();
                    statusM121 = "";
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("M121");
                    robotActionService.writeFalseM(201, false);
                    scheduledFuture00.cancel(false);
                    mapSF.remove("M121");
                } else {
                    robotActionService.getRobotM121();
                }
            }
        }, 100);
        mapSF.put("M121", scheduledFuture);
    }

    /**
     * 延时获取当前状态-通知抖料动作开始 M124
     */

    public void delayStatusM124() {
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {

            public void run() {
                if (statusM124.equals("01")) {
                    nextIntRobot();
                    statusM124 = "";
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("M124");
                    robotActionService.writeFalseM(204, false);
                    scheduledFuture00.cancel(false);
                    mapSF.remove("M124");
                } else {
                    robotActionService.getRobotM124();
                }
            }
        }, 100);
        mapSF.put("M124", scheduledFuture);
    }

    /*
     * ---------------------------------电机状态判断-----------------------------------------
     */

    /**
     * 延时获取当前状态 - 电机初始化完成 -delayMotorInitFinish
     */

    public void delayMotorInitFinish(StatusCallback callBack) {
        final int[] times = new int[]{0};
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {
            public void run() {
                // 判断 -- 当前电气位置是否到达
                if (motorHomeStatus.equals("1")) {
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("delayMotorInitFinish");
                    scheduledFuture00.cancel(false);
                    mapSF.remove("delayMotorInitFinish");
                    callBack.statusCallback();
                } else {
                    times[0]++;
                    if (times[0] > 5000) {
                        ScheduledFuture<?> scheduledFuture00 = mapSF.get("delayMotorInitFinish");
                        scheduledFuture00.cancel(false);
                        mapSF.remove("delayMotorInitFinish");
                        log.info("delayMotorInitFinish" + "已经超时，报警信号");

                    } else if (times[0] % 200 == 0) {
                        cmdHandleService.motorReady();
                        cmdHandleService.motorInit();
                    } else {
                        // 读取是否到达远点
                        cmdHandleService.motorArriveZero();
                    }
                }
            }
        }, 100);
        mapSF.put("delayMotorInitFinish", scheduledFuture);
    }

    /**
     * 延时判断-判断电机当前是否到位
     *
     * @param motorArriveLocation 电机需要到达的位置
     * @param callback            回调
     */
    public void stepComplete(int motorArriveLocation, StatusCallback callback) {
        final int[] times = new int[]{0};
        String motorLocation = "MotorReadLocation" + motorArriveLocation;
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {
            public void run() {
                DelayUtil.delayedExecution(1000, new Runnable() {
                    @Override
                    public void run() {
                        //判断误差区间
                        log.info("当前电气位置为:" + motorCurrentLocation + ",预计到达位置为：" + motorArriveLocation + ",当前偏差为:" + differentLocation);
                        /*
                            Need-添加信号判断
                         */
                        if (motorCurrentLocation >= (motorArriveLocation + differentLocation) - 1000
                                && motorCurrentLocation <= (motorArriveLocation + differentLocation) + 1000) {

                            ScheduledFuture<?> scheduledFuture00 = mapSF.get(motorLocation);
                            scheduledFuture00.cancel(false);
                            mapSF.remove(motorLocation);
                            callback.statusCallback();
                        }
                        /* 超时报警 */
                        times[0]++;
                        if (times[0] > 2000) {
                            ScheduledFuture<?> scheduledFuture00 = mapSF.get(motorLocation);
                            scheduledFuture00.cancel(false);
                            mapSF.remove(motorLocation);
                            log.info("已经超时，报警信号:" + motorArriveLocation);
                            /* 如果超时，则发出指令，让电机停止运动 */
                            cmdHandleService.motorStop();
                        }
                    }
                });
            }

        }, 100);
        mapSF.put(motorLocation, scheduledFuture);
    }

    /**
     * 获取传感器状态
     *
     * @param sensorKey key键
     * @param value     预期结果
     * @param callBack  回调
     * @param timeOut   超时时间
     */
    public void getSensorStatus(int sensorKey, int value, StatusCallback callBack, int timeOut) {
        /* p */
        final int[] times = new int[]{0};
        /* 将传感器的key转换为String类型的 */
        String key = String.valueOf(sensorKey);
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(new Runnable() {
            @Override
            public void run() {
                Byte sensor02 = pcbStatusDto02.get(sensorKey);
                log.info("判断当前传感器状态-" + sensorKey + "为" + sensor02 + "，预期结果为" + value);
                if (sensor02 == value) {
                    //sensorStatus.put(sensorKey,"");
                    callBack.statusCallback();
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get(key);
                    scheduledFuture00.cancel(false);
                    mapSF.remove(key);
                } else {
                    times[0]++;
                    if (times[0] > timeOut) {
                        ScheduledFuture<?> scheduledFuture00 = mapSF.get(key);
                        scheduledFuture00.cancel(false);
                        mapSF.remove(key);
                        log.info("已经超时，报警信号:" + sensorKey);
                    }
                }
            }
        }, timeOut / 20);

        mapSF.put(key, scheduledFuture);
    }

    public interface StatusCallback {
        void statusCallback();
    }

}
